package co.edu.unal.cg.splines;

import java.util.ArrayList;

import org.la4j.matrix.Matrix;
import org.la4j.matrix.dense.Basic2DMatrix;

import processing.core.PVector;

public class CardinalSpline extends Spline {

	float tension;
	ArrayList<PVector> controlPoints = new ArrayList<PVector>();

	
	public CardinalSpline(double stepSize){
		this(stepSize,0.5f);
	}
	
	public CardinalSpline(double stepSize,float tension){
		super(stepSize);
		if(tension>=0 && tension <=1) this.tension = tension;
		else throw new RuntimeException("The tension for a new CardinalSpline must be between 0 and 1");
	}
	
	
	public void addControlPoint(PVector position){
		controlPoints.add(position);
	}
	
	@Override
	public ArrayList<Matrix> getCoefficients() {
		if(controlPoints.size() < 4) throw new RuntimeException("A cardinal spline can only be constructed with 4 or more control ponints.");
		
		int n = controlPoints.size()-2;
		ArrayList<Matrix> coefficients = new ArrayList<Matrix>(n);
			
		for(int i=1; i < controlPoints.size()-2; i++){
			Matrix M = new Basic2DMatrix(3,4);
			
			PVector d = controlPoints.get(i);
			PVector c = PVector.mult(PVector.sub(controlPoints.get(i+1), controlPoints.get(i-1)), (1-tension)/2);
			PVector tmp =  PVector.mult(PVector.sub(controlPoints.get(i+2), controlPoints.get(i)), (1-tension)/2);
			PVector b = PVector.sub( 
							PVector.sub(
								PVector.sub(PVector.mult(controlPoints.get(i-1), 3),
											PVector.mult(c, 2)),
								PVector.mult(d, 3)),
							tmp);
			PVector a = PVector.sub(PVector.sub(PVector.sub(controlPoints.get(i-1),b),c),d);
								
			M.set(0, 0, a.x);
			M.set(0, 1, b.x);
			M.set(0, 2, c.x);
			M.set(0, 3, d.x);
			
			M.set(1, 0, a.y);
			M.set(1, 1, b.y);
			M.set(1, 2, c.y);
			M.set(1, 3, d.y);
			
			M.set(2, 0, a.z);
			M.set(2, 1, b.z);
			M.set(2, 2, c.z);
			M.set(2, 3, d.z);			
			
			coefficients.add(i-1,M);
			
		}
		
		return coefficients;
	}

}
